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Abstract 

This paper proposes new algorithms for attitude estimation and control based on fused inertial vector measurements using 
linear complementary filters principle. First, n-order direct and passive complementary filters combined with TRIAD algorithm 
are proposed to give attitude estimation solutions. These solutions which are efficient with respect to noise include the gyro bias 
estimation. Thereafter, the same principle of data fusion is used to address the problem of attitude tracking based on inertial 
vector measurements. Thus, instead of using noisy raw measurements in the control law a new solution of control that includes a 
linear-like complementary filter to deal with the noise is proposed. The stability analysis of the tracking error dynamics based on 
LaSalle’s invariance theorem proved that almost all trajectories converge asymptotically to the desired equilibrium. Experimental 
results, obtained with DIY Quad equipped with the APM2.6 auto-pilot, show the effectiveness and the performance of the proposed 
solutions. 


Index Terms 

Attitude Estimation; Attitude Control; Complementary Filters; Asymptotic Global Convergence; Almost Global Asymptotic 
Stability 


I. Introduction 

Most of traditional rigid body attitude control approaches given in the literature are based on feedback scheme using attitude 
estimation (see e.g. HI, 12, 0, 0, 0). Recently, some authors propose to use directly raw vector measurements to perform 
attitude control (see e.g. | 6 ], 0, 0). In fact, the explicit use of the attitude in the control law involves the determination 
of attitude from measurements provided by appropriate sensors. It is known that there are no sensors directly measuring the 
attitude but it can be determined from measurements in the body frame using suitable algorithms (see e.g. 0, DEL EL DE2, 
El). Almost all attitude control applications use measurement data from embedded Inertial Measurement Units (IMU). The 
capability of the rigid body to track desired attitude trajectories depends on the reliability of these sensors and the quality of 
measurements related to sensitivity to noise, bias, etc. To take into account the sensor imperfections, many techniques of attitude 
estimation and control were developed. For instance, as mentioned in the survey paper del the problem of attitude estimation 
is generally treated in two steps, estimation of the attitude with raw measurements and filtering. The most and widely used 
techniques in this case are based on extended Kalman filter del ed. Some other techniques are developed like the nonlinear 
observer given in del or based on unscented filter EL Most of these methods are computationally demanding and some 
of them, depending on used attitude representation na. suffer from topological limitations, double covering or singularities. 
Another class of techniques are based on complementary filters fl9l . GUI which are not so computationally demanding, see 
ED for comparison between complementary and Kalman filtering. 

Due to their simplicity and efficiency, the use of complementary filters to reconstruct the attitude continues to attract many 
researchers. A lot of them focus on low-cost IMU and attitude heading reference system AHRS [52- Nonlinear complementary 
filters designed on Special Orthogonal Group SO( 3) l23l and on the unit 3-sphere S 3 li24ll were used successfully to estimate 
the attitude. Modified complementary filters using only accelerometer and gyroscope measurements to estimate the orientation 
was presented in j25l . Another recent work has used the inverse sensor models and complementary filters to develop a high- 
fidelity attitude estimator |26l . As mentioned in |27l . traditional attitude solutions use directly raw vector measurements to 
compute the attitude data after that the observer is used to estimate the attitude. E3 proposed a reverse strategy by combining 
a vector-based filter with an optimal attitude determination algorithm, in which the distortion of noise characteristics is avoided. 
A new interesting class of globally asymptotically stable filters for attitude estimation was obtained. The vector-based filter 
was designed as a Kalman filter using Linear Time Variant (LTV) representation of the nonlinear kinematic equation. Even 
if experimental results presented in ll27l are very good, the theoretical drawback is the fact that the observability conclusions 
were given for the LTV reformulation of the original nonlinear system and not explicitly on the non linear system. 

Inspired by approach given in ll27l . this paper presents firstly globally asymptotically stable filters for attitude estimation 
based on high order linear complementary filtering. The gyro-bias estimation is also considered. Two forms of filter, termed 
“direct” and “passive”, are designed similarly as the work presented in l23l . The passive form is less sensitive to noise as 
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claimed in ll23ll . Moreover, the approach proposed here is completely deterministic as it is based on linear complementary 
filters followed by TRIAD algorithm for the attitude estimation. As a matter of fact, the TRIAD is the deterministic attitude 
estimation algorithm par excellence as claimed by l28l . Although it was proved that TRIAD is less accurate than other optimal 
approaches 1281 . we show throughout this work that it is possible to obtain higher quality of the attitude estimation when this 
approach is used. 

The quality of IMU measurements is much degraded by the phenomenon of vibrations of the real system. Frequently, the 
implementations of some attitude controllers using directly raw vector measurements are confronted with this phenomenon. 
Therefore, we propose to use a new filter to improve the performance of the attitude tracking controller. The proposed attitude 
controller is based on the filtered vector measurements instead of the raw ones, while ensuring an almost global stability 
without using “attitude measurements”. 

The result presented in this paper extends those from l29l . The first contribution of this work is the extension of the global 
convergence of the direct complementary filters to the case of n-order. Also, we propose general n-order passive filters, where 
we obtain the global asymptotic convergence to zero of the estimation errors. This constitutes our second contribution. Another 
contribution is the design of a new control law based only on inertial and rate-gyro measurements to control the attitude of 
a rigid body without using “attitude measurements”, for which an almost global stability is given. All our contributions are 
validated by experiments on the DIY drone Quad-copter lf30l . 


II. Preliminaries 


A. Mathematical background and Notations 

Consider a rigid-body moving in 3D space with orthonormal body-frame {B} fixed to its center of gravity and denote by 
{X} the inertial reference frame attached to the 3D space. Attitude of the rigid body represents the relative orientation of 
the {£>} with respect to {X}. It can be represented using several mathematical models. Representing the attitude by rotation 
matrix R, provides an unique, global and non singular parametrization of the orientation m. The rotation matrix is an 
element of the special orthogonal group 50(3) with 50(3) = {R £ R 3x3 | R T R = RR T = I 3l det (i?) = 1} where I 3 
is the 3x3 identity matrix. The associated Lie algebra denoted by so(3) is the set of skew symmetric matrices such that 
so(3) = {A £ R 3x3 | A = — A T }. Denote by S the Lie algebra mapping from R 3 -A so(3) which associates to x £ R 3 the 
skew-symmetric matrix S(x), such that 



0 

-X z 

Xy 


•Ex 

S(x) = 

1 

0 

0 

and x = 

1 

1 _ 


For any two vectors x, y £ R 3 and rotation matrix R £ 50(3), the following identities hold: 

{ S(x}y =xxy = -S(y)x , 

S(S(x)y) = S(x)S{y) - S(y)S(x), 

S(x) 2 = xx T — x T xI 3 , 

S(Rx) = RS(x)R t , 


(1) 


( 2 ) 


where x denotes the vector cross product. 

Another global and non singular parametrization of the attitude is described by unit quaternion Q which is an element of unit 
sphere § 3 = = (go,g T ) T , go G R, q £ R 3 , ?o + q T q = lj- The multiplication of two quaternions P = (po,p T ) T and 


Poqo ~P q 
Poq + q 0 p + px q 


Q = (go, q T ) T is denoted by “©” and defined as PqQ = 

L f 

we have Q © Q~ x = Q~ l © Q = (1, 0), where Q~ l = (q 0 , —q T ) T . 

Both Q £ S 3 and R £ 50(3) are related to each other through the mapping 7Z : § 3 
formula as follows: 

7Z(Q) =I 3 + 2 q 0 s(q) + 25(g) 2 


and for any unit quaternion Q = (go, g T ) T , 
50(3) by the Euler-Rodriguez 

(3) 


If n is a positive integer, set e n = (0, • ■ ■ ,0, l) r . To every 7 = (71,..., 7„) £ R n , we associate the polynomial 

n 

P 7 ( S ) = S " + ^7fcS n - fe , 

k =1 


( 4 ) 
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and the companion matrix A 7 


/ o 
0 


-A<y - 


1 0 

0 1 

: 0 


0 \ 


(5) 


: : : 0 
0 0 ••• 0 1 

\ -In 'Yn—l ■ ■ ■ -72 -71 / 

whose characteristic polynomial is P 7 . Use tt : K n — > K" _1 to denote the projection onto R™ _1 i.e., 7r(7) = (7 1; • • • ,7„_i). 
Define the following subsets of M n , 

H n = {7 e K” | P 7 Hurwitz} , H n = {7 € H n | ^(7) e K n -i} ■ 


The proof of the following lemma is defereed in Appendix. 

Lemma 1. If n is a positive integr, then 'H„ is not empty. 

Note 1. Let E € R( nxn '> and a(E ) = {Ai,...,A n } its spectrum, where A/, l = 1... n are the eigenvalues of E. Let 
Ik £ R( kxk \ k integer, be the identity matrix. Then, the spectrum of the Kronecker product of E by I k , E<g) I k £ 
is equal to a(E) according to Theorem in page 245 of f3H . In particular, E 0 I k is Hurwitz if and only E is. 


B. Attitude kinematics, Dynamics and Assumptions 

The rigid body rotational motion can be described by its kinematic and dynamic equations. Using the rotation matrix 
representation, the rigid body attitude is governed by the following kinematic equation 

R(t) = R(t)S(u(t)), (6) 


where u>(t) being the angular velocity of the rigid body expressed in {B}. Equivalently in term of unit quaternion, we can 
have Q(t) = \Q(t ) 0 u>(t) with uj[f) is the pure quaternion defined by Q(t) = (0,w(f) T ) T , which gives 


Q(t) 


4o{t) 


-\q T (t)u(t) 

q(t) 


. + S(q(t)))u(t) _ 


(7) 


Now, given a constant vector r in inertial {P}, then its corresponding vector in the {£>} is given by 6(f) = R T (t)r. Thus, 
using & one can get the following reduced attitude kinematics 


6 (f) = -S{u(t))b(t) ( 8 ) 

By considering applied torque r(f) to the system expressed in {£>}, the rigid body simplified rotational dynamics is governed 
by 

Jw(f) = — 5(w(f)) Jw(f) + r(f), (9) 

where J £ K 3x3 is a symmetric positive definite constant inertia matrix of the rigid body with respect to {£>}. 

Consider the following rate-gyros model 

W m (f) = w(f) + T), (10) 


where w m (f) is the measured angular velocity and 77 is the real unknown gyro-bias. 

Along this work, we use the following assumptions : 

Assumption 1. We assume that, if we have m measured vectors (t), i = expressed in { B }, corresponding to m 

inertial constant vectors ri, i = 1, ...,m expressed in {T}, then at least two of them are non-collinear. 


Assumption 2. We assume that the real unknown gyro-bias p is bounded and constant (or slowly varying), such that f] = 0. 
Moreover, we assume that we are dealing with bounded measured angular velocities u> m (-), implying that the real angular 
velocity ur(-) is bounded as well. 

Using the reduced attitude kinematics and the model of the rate-gyro we can write the following system 

j hi = —S(u] m — p)bi 

\ 7) = 0. 

where i = 1 Note that, in all what follows the indices i = 1 denote the number of the used inertial vectors. 
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C. Complementary linear filter-based attitude estimation approach 

The sensor-based attitude estimation approach ED is consisting of two processes: i) filtering sensor measurements, and 
ii) determining attitude. Inspired by this approach, we propose a structure based on complementary linear filter rather than 
sensor-based filter method. Indeed, complementary filters give us a mean to fuse multiple heterogeneous independent noisy 
measurements of the same signal that have complementary spectral characteristics |[23l . By developing a high-fidelity and 
simple algorithm for attitude estimation, the proposed structure must allow the possibility of using high order filter which leads 
to better performance. 

Using the reduced attitude kinematics ([8]l, the complementary filter model for fusing the measured inertial vector frj(f) and 
gyros measurements ui m in order to get estimate h,(f) is shown in Figure [I] where the notion of complementary filter is 
achieved if the following condition is satisfied 

H u (s) + sH 2i fs) = 1, i = 1, - ■■ ,m, (12) 

where Hu(s) is a low-pass filter and sH 2 i(s) is a high-pass filter. 



Figure 1. Classical form of complementary filter 


From the structure of the complementary filter given in Figure [ij the estimate h, of the state h, by fusing measurements of 
i th inertial direction vector and gyro measurements can be write as 

bi = Hu(s)bi + H 2 i{s)bi, i = (13) 

Now, for the determination of the attitude, the complementary filter can will be followed by a TRIAD algorithm fTOl . Despite 
the fact that TRIAD is known less accurate than other statistical algorithms based on minimizing Wahba’s loss function (28l . 
we will show that we can obtain good results by using fused data. The choice of TRIAD algorithm is justified by the fact that 
optimal algorithms are usually much slower than deterministic algorithms m, 128). 

The first problem addressed in this work is the design of an attitude and heading reference system using the concept of sensor- 
based attitude estimation approach ED- The goal is to proof that it is possible to obtain a structure based on complementary 
linear filter with a globally asymptotic convergence. The filtered data will be used by a TRIAD for attitude determination as 
explained before. 

The second problem addressed is to proof that the use of estimated measurements by complementary filters can achieve 
attitude tracking with an almost global stability. 

III. Design of High Order Direct and Passive Filters with Gyro-Bias Estimation 

The principle of the “classical form ” of complementary filters is based on the data fusion of measurements of inertial direction 
vectors and gyro measurements as depicted by the scheme of Figure |T] This scheme can be reformulated in “feedback form ” 
as shown by Figure [2] Furthermore, according to the manner of offsetting the nonlinear term, we can obtain two structures 
of the complementary filter. The first one is termed “direct linear complementary filter” and the second one termed “passive 
linear-like complementary filter” . Indeed, in the first one, the offsetting of nonlinear term uses direct raw measurements as 
shown in Figure [2] while in the second one, the filtered measurements are used as depicted in Figure [3] 


From the equivalence between the “classical form” and the “feedback form ”, one can get 

i = 1,..., to , 


Hu (s) = C ' 1 ^] , , H 2l (s) = 


s + Ci ( s ) ’ 


s + Ci (s) : 


( 14 ) 


where Cfs) represents the compensator term in the feedback form. From (14i, we can write the compensator term as 















CO 
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Figure 2. Direct linear complementary filter 


CO 



Figure 3. Passive linear-like complementary filter 


Ci{a) 


sH u (s) 

l-H u ( S y 


i = 1 , to . 


05) 


The design of the compensator Ci(s) can be achieved by choosing the adequate filter order for improving the quality of 
estimation. Consider now, for *=!■■• , to, the general n- order transfer function Hu(s) by first taking T, £ 'H n and setting 


TT ( \ Yin 

Hii(s) = 


7 h 

Pri (s) ’ 


where Py. ( s ) and 7 j„ are defined by 0 - Using ( p~5| ) one can get 

Ci(s) = 


Pn(ri)(s) 


i = 1 , to . 


06) 


(17) 


A. High-Order Direct Linear Complementary Filters 

Consider System (111 and the block diagram of the direct form in Figure [2] with compensator C,(s) given by (17 i for 
i = 1,..., to. Then, the closed-loop dynamics with gyro bias estimation for any n-order is given for i = 1,..., to by 


" { ” X) = - Efe=l 'YikXi 1 k 1} +7 in{bi-bi), 
= —S(u> m — fj)bi + Xi, 


(18) 


= 5(60v< 


where is the j —th derivative of X{ with = 27 , 7 ^, i = 1,..., to, k = 1,..., n are components of T x £ T-L n , is a real 
positive definite diagonal matrix gain and v t is a vector to be defined later. 

Define the observation errors 


bi = bi — bi, i = 1 , 
rj = rj-fj , 

then using ([ 8 ]) and (18 l-(pO|), yield the following error dynamics 

~vn— 1 


(n— 1) v-^n— 1 (n—k—1) 

' = - Ek=i 7 ikx\ 

= -S(bi)rj - Xi, 


linbi 


x l 

% 

V = - T dT,?=i S (bi)vi. 

By the evaluation of the time derivative of the first equation of ( |2T| , one can rewrite © as 

/ x l n) = - Efe=i 7ifc*i n_fe) -1inS{bi)rj 


(19) 

( 20 ) 


( 21 ) 


7 


= -r d £^i S(bi) 


( 22 ) 
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Now, consider the new state vector z, £ M 3n , i = 1, ...,m such as zf = [x T ,x T , ■ ■ ■ ,x^ n 1 ) T ] and define the vectors Vi 


to be 

Vi = BfPdiZi, i = 1,m. 

(23) 

One can rewrite (|22|) as 

( Zi(t) = A di Zi(t) + B di S(rj)bi, 

[V = -T d EZi S(bi)BlP diZi , 

(24) 

where i = 1,..., m, the Hurwitz matrices A di = Ay, <S> I d £ K( 3nx3n ) (Ay, is debned by ([ 5 ])), B d i = Ai n e n ® I 3 £ K 3rax3 and 
the matrices P d i £ R ! " 3nx3T 7, i = 1, are real symmetric positive debnite solutions of the following Lyapunov equations 

for given symmetric positive debnite matrices Q d i 

-A-di Pdi “ 1 “ Pdi -A-di Qdit ^ 15 • • • 5 ^ 

(25) 


We can now state our first result. 


Proposition 1. Consider the filter © and ( |2i| ), under Assumptions [7] and^in subsection \II-B\ Then the errors © and © 
converge globally asymptotically to zero. 

Proof: Consider the following Lyapunov function candidate 


V 1 =Y t ^PdiZi + r{ r T~ d X V 


(26) 


where P<n £ R( 3nx3n ) ; i = 1, ...,m is given by (G5Jl. The time derivative of (26 1 in view of <24 1 is given by 


Vl = YZ, 1 { Z f P diZi + Z PdiZi ) + rf'T d 1 t], 

= E™ i (zf ( A di p di + PdiAu ) Zi + 2 zfP di B di S(p)bi) - Y?=i S(bi)BT.P diZi 


using (25) and the fact that rj 1 - S(bi)BT.p di Zi = zf PdiBdiS(rf)bi, then 

m 

Vi = - ^2 zf Q di Zi ^ 0. 


(27) 


Therefore z,- t and 777 are bounded and consequently by using (24 1 , z, and 77 , are bounded. The evaluation of the second derivative 
of ( 261 in view of (24 1 gives 


Vi = - Y, zf (AlQ di + Q di A di ) Zi + 2zfQ di B di S(b i ) V , 


(28) 


2 — 1 


which is clearly bounded. By Barbalat’s lemma, lim V\ (t) = 0 and consequently lim Zi(t) = 0. Then, according to (21), one 

t—foo t—too 

can obtain lim 6 t (f) = 0. The second time derivative of z, is given by 

t—too 


Zi = A di ( A di z di {t ) + B di S(bi)rj) + B di S(S(bi)tv)r 7 + B di S(bi)r), 


(29) 


where all terms are bounded. Thus using Barbalat’s lemma, lim 27 (f) = 0. Therefore, using |24| and lim z,(t) = 0, one can 

^ t—¥ OO _ I I —L-A-oo 

conclude that B d iS(bi)r) converge to zero and equivalently lim S(bi(t))r](t ) = 0. Under Assumption [ll one can conclude that 

t—fOO 

lim 77 (f) = 0 . ■ 

£—>•00 

Remark 1. Substituting the value of n by 1 in ( p~ 8 | >, and after some manipulations, one can obtain the first order direct filter as 

| k = -S(u} m -7))bi + 'Yi 1 (bi-b i ) 

1 b = YYZi s(b,.% 


B. High-Order Passive Linear-like Filters 


In the passive form, the design of the complementary biter is performed by injecting hltered measurements for offsetting 
nonlinear term as shown in block diagram of Figure [5] with a compensator C)(s), i = 1, • • ■ , m, debned by (17 1 . Then, we 
propose the following new n-order passive form with gyro bias estimation 


f = - Efc=J 7ifcZi n_fc_1) +Ain{bi ~ &»), 

S U = -S(uj m - f))bi + Wi > 

l V = -V P YiLi s ( b i)bi, 


( 31 ) 
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where i = 1 , m, x!' j :>> is the j th order derivative of X{ with = Xi, "fik, i = 1 , m, k = 1 ,(n — 1 ) are components 
of 7 r(Tj) for Ti £ H n , T p is a real positive definite diagonal matrix gain and Wi are given by 


Wi = B pi P pi Xi, 


with X, £ 


\ i = 1, ...,m such as Xj = \x T , 


• T 
X , 


, x} n 2 ^ T ], allowing to rewrite (31 1 as 


(32) 


f Mt) — ApiXi (f) + B pi (bi — hi), 

\ Si = -S(u m - fj)k + B^PpiXi, (33) 

l v = - r pEili s '( 6 *)^. 

where the Hurwitz matrices 4 pi = Aj-pr,) <E> Id £ ]R( 3 ( rl - 1 ) x3 (”- 1 )) {A n (r,) is defined by S), see Note [l] for A p ,- Hurwitz) 
and the matrices B pi = 7 in^( n -i)®Id €E M 3 ( n_1 ) x3 and the matrices P pi G |ji(3(n-i)x3(n-i)j^ = l ; ...,rn, are real symmetric 
positive definite solutions of the following Lyapunov equations for given symmetric positive definite matrices Q p i 


A 1 P 

- ti pi r pi 


T BpiA p i — (J 


pn 


(34) 


We now state our second result. 


Proposition 2. Consider the filter p7| ), under Assumptions [7] and [2] in subsection \II-B\ Then the errors and ( |20| ) converge 
globally asymptotically to zero. 


Proof: First let us evaluate the error dynamics of ( |33| . Using © and (fl9|.(p0|), one can get 

Mt) — A pi Xi(t) + Bpibi, 


= -S{bi)rj + 5(Si)(w +v)~ B T pi P pi X u 


bi 

rj = -YpY?=i S ( b i) b . 

Consider now, the following Lyapunov function 


^2 = E X ? p vi x i + E + ^p% 


i =1 


i =1 


the time derivative of ( [36] ) in view of ( [35) is given by 

^ = EZi ( x H^iPpi + PpiO x i), 

since A pl . i = 1,... , to is Hurwitz, then the Lyapunov equation ([34]) holds. Therefore, one can obtain 


V 2 = -'52 x ?Q P i x i^0. 


i— 1 


II-B 


(35) 


(36) 


(37) 


Xi, bi and r/ i are 


Therefore, Xi, bi and rji are bounded and consequently from (35 i and Assumption |t] in subsection i _ 

also bounded. The rest of the proof is similar to the proof of Proposition [7] It is easy to verify that V 2 is bounded. Thus 
using Barbalat’s lemma, lim V 2 (t) = 0 and consequently lim Xft) = 0. In addition, Xi are bounded, then lim X^t) = 0 

t—> OO t—too OO 


and using (35 1 , lim &j(f) = 0. By a standard reasoning by contradiction, one gets that lim 6 j(f) = 0. Using this fact and 


therefore lim 


t—*oo~ _ _ t—> OO 

S(bi)r) = 0. Under Assumption fl| one can conclude that lim 77 (f) = 0. 

£—>•00 


§, 


Remark 2. Substituting the value of n by 1 in < [3~i~[ >, and after some manipulations, one can obtain the first order passive filter 
as 


bi = -S(uj m - fj)bi + 7 n(bi - hi), 
V = r 2 Y^iS(bi)bi, 


(38) 


IV. Attitude tracking using complementary filter principle 

We propose thereafter a new control law that use only filtered inertial vectors and rate gyro measurements to track the 
desired attitude, without using “attitude measurements”. The filtered inertial vectors are obtained using a new filter based on 
first order direct complementary filter. 











A. Controller Design 

First, let us define the orientation error by R(t) = R{t)R^{t) which corresponds to the quaternion error Q(t ) = Q(t) 0 


Qi(t) = 


qo(t) 

q(t) 


G S 3 whose dynamics is governed by 


qo(t) 


-\q T (f)R d {t)u{f) 

q{t) 


\ {qo{t)I d + s(q{t))) R d (t)u(t) _ 


(39) 


where R d (t) is the desired rotation matrix and it’s equivalent unit-quatemion is Q d (t). The angular velocity error is defined 
by 

Q(t)=ui(t)-u d (t), (40) 

where u> d (t) is the desired angular velocity. We now propose the following new filter designed for the control problem 

bi(t) = -S(u)bi + oti{bi(t) - Sj(f)) + S(uj d ){bi{t) - 6*(f)) + 5iS(bf(t))Q(t), (41) 

where a, >0, 6, >0 (i = 1 ,m) and the following new control law 


-(f) = S(u(t))Ju(t) - JS{u d (t))u{t) + Jw d (t) + piS(bf(t))bi(t) - kJQ(t), 


(42) 


i —1 


where Qj > 0, i = 1,.. ., to , k > 0 and bfit) is obtained by (41 1 . 

Using <§■ (|39ll, (|4 1 1 and ([ 42 J and define the new variables u> = R d Cj and b t = IlJ), one can get the following closed 


loop dynamics 


bi(t) = — SiS(ri(t))u>(t), 

qo (t) = -\q T {t)u(t), 

q(t) = ^(qo(t)I d +S(q(t)))u{t), 

w(t) = —2(q 0 I d — S(q))Wq — PiS(, r i)t>i(t) — kui(t), 


(43) 


where W = — IT' is a positive define matrix (see Lemma 1 and Lemma 2 |[ 8 l). 

Let us define the state 0 := (fq,..., b m , Q, w). The closed loop dynamics (43 1 can be rewritten as 0 = G(0) such that 
0 G A and A := R 3m x S 3 x R 3 , and define the following positive radially unbounded function : V 3 : A —> R. 

m 

Pi T.Tu\l 


^3(0) = T b ^ ^ bi + 2 q(.t) Tw q(t) + w(f) T w(f). 
1=1 


(44) 


Theorem 1. Consider System ([7])-([9| and the control law (42) with the observer given by m Under Assumption [7] in 


0f — (°3, •••; 03j 


43 1 are defined by 


' ±1 ' 

0 

) 0 ), 02, 3 ,4 — (03) - 03) 

1 - 1 

°l 

1 _ 1 


, 0 ), 


where Vj , j = 1, 2, 3 are the eigenvectors of W. 

(2) The equilibria ©J are asymptotically stable with a domain of attraction containing the set 


for 0 ^ and 


C+ := {0 e A | U 3 (0) < 4A min {W) and q 0 > 0} , 
C~ := {0 G A | U 3 (0) < 4A mm (W) and q 0 < 0} , 


for 0 X , where A m in{W) is the smallest eigenvalue ofW. 

(3) The equilibria 0 ^ 3 4 are locally unstable and 0^ are almost globally asymptotically stable. 

Proof: The proof of the first item is similar to the proof of Theorem [I] presented in H32| . Recall that the closed loop 
dynamics ( [43] ) is autonomous, therefore it is possible to use LaSalle’s invariance theorem to proof the second item. Note that 
the time derivative of (44) using (|43j is given by U 3 (0) = —fcu)(f) T w(f) — 1 a i P fbi{t) T bi{t) < 0 and the proof of item 

(2) will be similar to the proof of Theorem 1 presented in fl32l . 

(3) Let us proof that the equilibria 02 3 4 are unstable. Since the only difference between these equilibria is the value 
of the eigenvector, the proof is given only for O j' G A . The other cases will be similar. To do this, we consider (-)) := 
(6*,..., lf rn . Q*, w*) a neighborhood of (-) 2 h (arbitrary close) and since the function V 3 is non-increasing, it suffices to prove 
that 4 ( 3 ( 02 ) — 4 ( 3 ( 0 ^) < 0. Let us use the following change of variable 
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(45) 


Using (451 and the fact that Wv i = Aii/i (where Ai is the eigenvalue associated to the unit eigenvector v\ of W), one can 
evaluate D = — V 3 (@J) as follow 


m 

D = y ^bfb* + u* t 0 j* + 4A(*g - 1) - 4x T S(v 1 )WS(v 1 )x, 
S i 


(46) 


If we take x close to v 2 such that x = ev 2 , where e > 0 sufficiently small, the unit quaternion constraint gives x/ = 1 — £. 
In this case, one can gets D = J2x= i ^b* T b* + Co* T ui* - 4Ai £ 2 which means that if £ 2 > jA 1 jrb* T b* +u)* T ui*^ 
then D < 0. As a result, there exist 02 arbitrary close to (-) 2 such that V/((-))) < V 2 {(~) 2 ) and since the function V 3 is non 
increasing, it is clear that 0^ is unstable. Similarly, all equilibria ©23 4 are unstable. Finally, in the state space A the set of 
unstable equilibria is Lebesgue measure zero. Therefore, almost all trajectories converge asymptotically to 0^. ■ 

Remark 3. In the case of stabilization (u>d = 0), the control law (|42) with the filter ([41) can be modified to get 


b si (t) = oti(bi(t) - b si (t)) - S(u}(t))bi(t) + 5iS(bi)u(t), 

m 

T s(t ) = y piS(bf(t))b si (t) - kw(t). 
i= 1 


(47) 

(48) 


V. Experimental results 

In this section, we present some experimental results showing the effectiveness and the performances of the proposed 
solutions. Experiments were done based on DIY drone project If30l . We have used the platform shown in Figure [4] It is a 
test-bench with DIY Quad equipped with the APM2.6 lf33ll autopilot used for indoor tests. The autopilot APM2.6 is based on 
Atmel ATMEGA2560-16AU using an external clock of 16MHz. The embedded system is equipped with Invensense’s 6 DoF 
Accelerometer/Gyro MPU-6000 and a 3-axis external compass HMC5883L-TR. The main loop operating frequency of the 
firmware is 100Hz. The acquisition of accelerometer and gyros measurements is similar to the main loop while the frequency 
acquisition of magnetometer measurements is 10 Hz (after an internal filtering). 

For experiments, 77 = [0, 0,1] T and r 2 = [0.434, —0.04,0.899] T are the gravitational earth vector and magnetic earth filed 
vector, respectively, expressed in North East Down “NED” reference frame and both normalized. To validate our results, two 
main experiments were done. The first one was made to evaluate the performance of our attitude observer using the well known 
Xsens MTi AHRS, as illustrated in Figure [5] In this experiment, the attitude measurements provided by the MTi is considered 
as a reference signal. The second experiment consists of the implementation of our attitude controller directly on the autopilot 
APM2.6. 


A. Attitude estimation 


As described above, the attitude measurements delivered by the Xsens MTi will be considered as a reference signal for the 
comparison of results. This reference is obtained with an internal Kalman filter implemented inside MTi. The explicit observer 
presented in fl23l with quaternion formulation was implemented and will be termed as “MHP" observer. 

Remark 4. For simplicity and implementation consideration, only the first order “Direct” and “ Passive” filters given by pO) 
and (38 1 were implemented using first order Euler integration, where we take i = 1, 2, b\ = a = [ a x a y a z ] 7 (m/s 2 ) for 


accelerometer measurements and b 2 = m = [ m x m y m z ] T ( normalized) for magnetometer measurements. 

For implementation, the following gains were chosen: 711 = 721 = 1 and A = 1' 2 = 0.003/,/ for both two filters while 
for “MHP” observer, the gains presented in ll23l were used : kp = 1 and ki = 0.3. The measured initial attitude condition 
given by MTi was Q( 0) = [0.998, —0.031, —0.029, — 0.046] T , which was used as initial condition for “MHP” observer 
and the equivalent initial conditions for “Direct” and “ Passive” proposed filters were a(0) = [0.771, —0.796, 9.652] r and 
m(0) = [0.049, 0.016, — 0.263]For reporting results, we first consider the performance of the data fusion obtained by 
implemented complementary filters. Then, figures [6] and [7] show experimental results for the direct and passive filters. One can 
observe that the two complementary filters have similar performance which corroborates the fact that asymptotic stability were 
demonstrated for both filters. As explained before, the passive filter is less sensitive to noise. This can be illustrated in Figure 
[ 6 )(c). Note that the raw magnetometer measurements are not very corrupted by noise as illustrated in Figure [7] and this due 
to the fact that they were already filtered inside the MTi. Thereafter, the outputs of theses filters are used to estimate attitude 
using TRIAD algorithm as illustrated in Figure [ 8 ] In this figure, the estimated attitude is compared to that obtained with the 
raw measurements. The comparison presented in Figure [9] illustrate the effectiveness of the proposed observer compared to 
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Figure 4. Test-bench DIY Quad 



Figure 5. The Inertial Measurements Unit Xsens mounted on the test-bench 


Kalman filter (implemented inside MTi) or “MHP” observer. In Figure |T0} the gyros bias estimation from both observers is 
shown and both two observers give roughly similar results. 












11 


w 




Figure 6. Complementary Accelerometer filters experimental results 





Figure 7. Complementary Magnetometer filters experimental results 


B. Attitude stabilization 

For this test, we considered for simplicity and without loss of generality the special case of stabilization of attitude. The 
experiment was done using the test-bench shown in Figure [4] The controller ( |48j ) was implemented using the following notations 
and parameters : Rd(t) = I, which means bf = rq and b* = r 2 \ b 1 = a (normalized), b 2 = m (normalized) are the estimates 
of the inertial vector measurements given by the accelerometer and magnetometer, respectively; ui(t) (rad/s) is the rate gyro 
measurements; p\ = 1.66 and p 2 = 0.1161 (for the axis x and y), and pi z = 0.05 and p 2z = 0.03 (for the z axis); The 
damping k = 0.2621 and the filter gains a\ = 6 and a 2 = 10. 

The main loop for attitude stabilization is running at 100Hz. At each loop the measurements of accelerometer and 
magnetometer are normalized after the execution of the observer ( [4l~| ). Due to the poor quality of magnetometer measurements 
the gains corresponding to 2 axis are chosen small. Therefore, the stabilization is done around x and y axis only. Then, 
starting from an arbitrary measured initial condition in Euler angles (cfi,0,ip) = (—18.478,41.192,2.847)°, the evolution of 
normalized inertial measurements vectors, torque and Euler angles are shown in Figure [IT] We can see that after transient time. 
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Figure 8. Attitude estimation experimental results for the proposed observers 
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Figure 9. Attitude estimation experimental results comparison 
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Figure 10. Rate gyro bias estimation experimental results 
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Figure 11. Attitude stabilization experimental results 


the normalized measurements vectors a and to converge to the desired values bf = [0, 0, l ] 7 and b% = [0.434, —0.04, 0.899] T . 
Consequently according with the attitude estimate, this corresponds to the roll and pitch angles close to zero which confirms 
the stabilization of the platform. We can also observe that control torque is smooth without noise through the use of the 
complementary filter. 


VI. Conclusions 

Due to its importance and despite the considerable number of solutions, the problem of attitude estimation and control is still 
relevant. This paper presents High order “Direct” and “Passive” linear-like complementary filters for attitude and gyro-rate 
bias estimation. Using Lyapunov analysis, the proposed solutions ensure global convergence. Another novelty of this work lies 
in the proposition of new control law for attitude tracking problem, in which the principle of data fusion is used. Only filtered 
inertial vectors and rate gyro measurements were used in the control law, without using “attitude measurements” and ensuring 
an almost global stability. To show the efficiency and performance of the proposed solutions, a set of experimental tests were 
performed based on DIY drone Quadcopter, equipped with APM2.6 autopilot. The passive second order filter can be of great 
help. Indeed, in future work, this filter will be used to enhance the low sampling frequency of magnetometer measurements 
compared to that of accelerometer. 


Appendix 


Proof of Lemma 1 

Showing the thesis amounts to exhibit an example. For that purpose, consider 7 = (C 7 l a , )i <;<„ G M ra , where n is a positive 
integer, a a positive real number and the C l n are the binomial coefficients. Then P 7 (s) = (s + a) 71 implying that 7 G 77 n . It 
remains to show that 7 G 7 ~L n . One clearly has that P^M = (P 7 (s) — P 7 (0))/s and thus the roots of Pn-( 7 ) are the non zero 

roots of (s + a) n — a n . Every root z of the previous polynomial verifies that + 1)" = 1 and then z n +1 = where 

j 2 = —1 and k = 0, ...,n — 1. It yields that Re{z) = a(cos(^ 1 ) — 1), which is negative only if k ^ 0 and in the latter case 

z = 0. One deduces that all the roots of P„-( 7 ) have negative real part, i.e., P n M is Hurwitz and thus 7 G 77.„. 
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